/**
 * @file:          LESO.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.08.06
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.08.06,01:33:48
 */

/* Include Files **************************************************************/
#include "LESO.h"
#include "common.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/
void LESO_Init(struct LESO *self)
{
    self->Run = LESO_Run_td_filt;
    self->Init_z1z2z3 = LESO_Init_z1z2z3;
    self->UpdateParam = LESO_UpdateParam;

    self->td.Init(&self->td);
    self->fe.Init(&self->fe);
    self->fe1.Init(&self->fe1);
    self->UpdateParam(self);
}

void LESO_Run(struct LESO *self)
{
    if (self->param_en != 0)
    {
        self->z1_last = self->out_z1;
        self->z2_last = self->out_z2;
        self->z3_last = self->out_z3;

        self->err = self->z1_last - self->in_y;
        if (self->param_single_turn_mode != 0)
        {
            if (self->err > BM_PI)
            {
                self->err -= BM_2PI;
            }
            else if (self->err < -BM_PI)
            {
                self->err += BM_2PI;
            }
        }

        self->fe.Run(&self->fe, self->err);
        self->fe1.Run(&self->fe1, self->err);

        self->out_z1 = self->z1_last + self->param_h * (self->z2_last -
                                                        self->beta1 * self->err);
        self->out_z2 = self->z2_last + self->param_h * (self->z3_last -
                                                        self->beta2 * self->fe.out +
                                                        self->in_u * self->param_b0);
        self->out_z3 = self->z3_last + self->param_h * (-self->beta3 * self->fe1.out);

        // self->out_z1 = fmodf(self->out_z1, BM_2PI);
        if (self->param_single_turn_mode != 0)
        {
            if (self->out_z1 >= BM_2PI)
            {
                self->out_z1 -= BM_2PI;
            }
            else if (self->out_z1 < 0)
            {
                self->out_z1 += BM_2PI;
            }
        }
    }
    else
    {
        self->out_z1 = self->in_y;
        self->out_z2 = 0;
        self->out_z3 = 0;
    }
}

void LESO_Run_td_filt(struct LESO *self)
{
    if (self->param_en != 0)
    {
        /* td */
        self->td.in_v = self->in_y;
        self->td.Run(&self->td);

        /* eso */
        self->z1_last = self->out_z1;
        self->z2_last = self->out_z2;
        self->z3_last = self->out_z3;

        self->err = self->z1_last - (self->td.out_x1 + self->param_k0 * self->td.param_h * self->td.out_x2);

        if (self->param_single_turn_mode != 0)
        {
            if (self->err > BM_PI)
            {
                self->err -= BM_2PI;
            }
            else if (self->err < -BM_PI)
            {
                self->err += BM_2PI;
            }
        }

        self->fe.Run(&self->fe, self->err);
        self->fe1.Run(&self->fe1, self->err);

        self->out_z1 = self->z1_last + self->param_h * (self->z2_last -
                                                        self->beta1 * self->err);
        self->out_z2 = self->z2_last + self->param_h * (self->z3_last -
                                                        self->beta2 * self->fe.out +
                                                        self->in_u * self->param_b0);
        self->out_z3 = self->z3_last + self->param_h * (-self->beta3 * self->fe1.out);

        // self->out_z1 = fmodf(self->out_z1, BM_2PI);
        if (self->param_single_turn_mode != 0)
        {
            if (self->out_z1 >= BM_2PI)
            {
                self->out_z1 -= BM_2PI;
            }
            else if (self->out_z1 < 0)
            {
                self->out_z1 += BM_2PI;
            }
        }
    }
    else
    {
        self->out_z1 = self->in_y;
        self->out_z2 = 0;
        self->out_z3 = 0;
    }
}

void LESO_Init_z1z2z3(struct LESO *self, float init_z1, float init_z2, float init_z3)
{
    self->out_z1 = init_z1;
    self->out_z2 = init_z2;
    self->out_z3 = init_z3;
    self->z1_last = self->out_z1;
    self->z2_last = self->out_z2;
    self->z3_last = self->out_z3;
}

void LESO_UpdateParam(struct LESO *self)
{
    self->beta1 = self->param_wp + self->param_ws + self->param_wd;
    self->beta2 = self->param_wp * self->param_ws +
                  self->param_wp * self->param_wd +
                  self->param_ws * self->param_wd;
    self->beta3 = self->param_wp * self->param_ws * self->param_wd;

    self->td.UpdateParam(&self->td);
}